/* 
 * File:   Robot.cpp
 * Author: hadrien
 * 
 * Created on 6 mars 2012, 23:26
 */

#include "Robot.h"
using namespace mrpt;
using namespace mrpt::math;
using namespace mrpt::utils;
using namespace mrpt::slam;
using namespace mrpt::random;
using namespace mrpt::system;
using namespace std;
using std::max;

/*************************************************************************
        Constructor
 *************************************************************************/
Robot::Robot(float TAU, float DELAY) :
m_pose(0, 0, 0),
m_odometry(0, 0, 0),
podoSinceLastObs(0,0,0),
v(0), w(0),
t(0),
use_odometry_error(false),
Command_Time(0),
Command_v(0), Command_w(0),
Command_v0(0), Command_w0(0),
cTAU(TAU),
cDELAY(DELAY),
m_minSensorDistance(0),
m_maxSensorDistance(0),
m_fieldOfView(DEG2RAD(180)),
m_stdRange(0),
sensorPoseOnRobot(0, 0, 0, 0, 0, 0),
m_stdYaw(0),
odoScale(0.001/SIZE_STEP_S){
    resetStatus();
    opts.modelSelection = CActionRobotMovement2D::mmGaussian;
    opts.gausianModel.a1 = 0;
    opts.gausianModel.a2 = 0;
    opts.gausianModel.a3 = 0;
    opts.gausianModel.a4 = 0;
    opts.gausianModel.minStdXY = 0;
    opts.gausianModel.minStdPHI = 0;
    obs = CObservationBearingRange::Create();
    obs->minSensorDistance = m_minSensorDistance;
    obs->maxSensorDistance = m_maxSensorDistance;
    obs->fieldOfView_yaw = m_fieldOfView;
    obs->fieldOfView_pitch = m_fieldOfView;
    obs->sensorLocationOnRobot = sensorPoseOnRobot;
}

Robot::Robot(const Robot &orig) :
m_pose(orig.m_pose),
m_odometry(orig.m_odometry),
podoSinceLastObs(orig.podoSinceLastObs),
v(orig.v), w(orig.w),
t(orig.t),
use_odometry_error(orig.use_odometry_error),
Command_Time(orig.Command_Time),
Command_v(orig.Command_v), Command_w(orig.Command_w),
Command_v0(orig.Command_v0), Command_w0(orig.Command_w0),
cTAU(orig.cTAU),
cDELAY(orig.cDELAY),
m_minSensorDistance(orig.m_minSensorDistance),
m_maxSensorDistance(orig.m_maxSensorDistance),
m_fieldOfView(orig.m_fieldOfView),
m_stdRange(orig.m_stdRange),
sensorPoseOnRobot(orig.sensorPoseOnRobot),
m_stdYaw(orig.m_stdYaw),
m_Axy_err_std(orig.m_Axy_err_std),
m_Aphi_err_std(orig.m_Aphi_err_std),
odoScale(orig.odoScale),
opts(orig.opts)
{
    /*opts.modelSelection = CActionRobotMovement2D::mmGaussian;
    opts.gausianModel.a1 = 0;
    opts.gausianModel.a2 = 0;
    opts.gausianModel.a3 = 0;
    opts.gausianModel.a4 = 0;
    opts.gausianModel.minStdXY = m_Axy_err_std;
    opts.gausianModel.minStdPHI = m_Aphi_err_std;*/
    obs = CObservationBearingRange::Create();
    obs->minSensorDistance = m_minSensorDistance;
    obs->maxSensorDistance = m_maxSensorDistance;
    obs->fieldOfView_yaw = m_fieldOfView;
    obs->fieldOfView_pitch = m_fieldOfView;
    obs->sensorLocationOnRobot = sensorPoseOnRobot;
    
}
/*************************************************************************
        Destructor
 *************************************************************************/

void Robot::simulateInterval(double At) {
    CPose2D dP, dPodo;
    double AAt, tt;
    tt = 0.0;
    AAt = 0.001; // Minimum step size

    while (tt < At) {
        t += AAt;
        tt += AAt;

        // Change velocities:
        // ----------------------------------------------------------------
        double t_transcurrido = t - Command_Time;
        t_transcurrido -= cDELAY;
        t_transcurrido = max(0.0, t_transcurrido);

        if (cTAU == 0 && cDELAY == 0) {
            v = Command_v;
            w = Command_w;
        } else {
            v = Command_v0 + (Command_v - Command_v0)*(1 - exp(-t_transcurrido / cTAU));
            w = Command_w0 + (Command_w - Command_w0)*(1 - exp(-t_transcurrido / cTAU));
        }

        // Simulate movement during At:
        // ----------------------------------------------------------------
        dP.x(v * AAt);
        dP.y(0);
        dP.phi(w * AAt);
        m_pose = m_pose + dP;

        // odometry:
        dPodo = dPodo + dP;
        if (use_odometry_error && (dP.x() != 0 || dP.y() != 0 || dP.phi() != 0)) {
            dPodo.x(dP.x() + m_Axy_err_std * randomGenerator.drawGaussian1D_normalized() * odoScale);
            dPodo.y(dP.y() + m_Axy_err_std * randomGenerator.drawGaussian1D_normalized() * odoScale);
            dPodo.phi(dP.phi() + m_Aphi_err_std * randomGenerator.drawGaussian1D_normalized() * odoScale);
        }
        m_odometry = m_odometry + dPodo;
    }

}

void Robot::resetStatus() {
    m_pose = CPose2D(0, 0, 0);
    m_odometry = CPose2D(0, 0, 0);

    t = 0;
    v = w = 0;
    Command_Time = 0;
    Command_v = Command_w = Command_v0 = Command_w0 = 0;
}

void Robot::setSensorParameters(const RangeSensorModel &sensor) {
    this->m_minSensorDistance = sensor.minSensorDistance;
    this->m_maxSensorDistance = sensor.maxSensorDistance;
    this->m_stdRange = sensor.stdRange;
    this->m_fieldOfView = mrpt::math::DEG2RAD(sensor.fieldOfView_deg);
    this->m_stdYaw = mrpt::math::DEG2RAD(sensor.stdYaw_deg);
    obs->minSensorDistance = m_minSensorDistance;
    obs->maxSensorDistance = m_maxSensorDistance;
    obs->fieldOfView_yaw = m_fieldOfView;
    obs->fieldOfView_pitch = m_fieldOfView;
    obs->sensorLocationOnRobot = sensorPoseOnRobot;
}

void Robot::movementCommand(double lin_vel, double ang_vel) {
    Command_Time = t;
    Command_v = lin_vel;
    Command_w = ang_vel;

    Command_v0 = v;
    Command_w0 = w;
}

void Robot::move(const Action& action, double time_interval) {
    movementCommand(action.velocity, action.turn_rate);
    simulateInterval(time_interval);
}

void Robot::sense(CActionCollectionPtr &actions, CSensoryFramePtr &observations, const CLandmarksMap &landmarkMap) {
    podoSinceLastObs = m_odometry - podoSinceLastObs;
    actions.clear_unique();
    observations.clear_unique();
    actions = CActionCollection::Create();
    observations = CSensoryFrame::Create();
    // Simulate observations:
    
    //printf("Simuate OBS\n");
    landmarkMap.simulateRangeBearingReadings(
                                             CPose3D(m_pose),
                                             sensorPoseOnRobot,
                                             *obs,
                                             true, // wheter to identy landmarks
                                             m_stdRange,
                                             m_stdYaw,
                                             0);
    //printf("Push back OBS\n");
    observations->push_back(obs);

    // Compute action
    CActionRobotMovement2D act;
    // Get odometry
    // robotKinematicModel.getOdometry(incOdo);
    //printf("Compute ODO\n");
    act.computeFromOdometry(podoSinceLastObs, opts);
    //printf("Insert ODO\n");
    actions->insert(act);

    podoSinceLastObs = m_odometry;
}



